import cv2
import numpy as np
import yaml

from binocular.binocular_ranging import Macrometer
from tool.process import Processor


def main(config):
    processor = Processor(config)
    macrometer = Macrometer(config)

    img_left = cv2.imread(config['image_path']['left'])
    img_right = cv2.imread(config['image_path']['right'])

    disparityLR, disparityRL = macrometer.calculate_disparity(img_left, img_right)  # 计算视差图

    print('disparityLR:',disparityLR[555][539])
    print('disparityRL:',disparityRL[555][539])

    masks_centroid_left = processor.detector_point_threshold(img_left)  # 图像增强处理
    masks_centroid_right = processor.detector_point_threshold(img_right)

    processor.show_disparity_image(img_left, disparityLR)  # 融合视差图和原图，显示并保存。
    processor.show_disparity_image(img_right, disparityRL)
    processor.show_center_point(img_left, masks_centroid_left)  # 将masks_centroid_left的数据显示到img_left上

    # 得到三维点云
    point_cloud = macrometer.calculate_3d_coordinate(disparity=disparityLR)

    distance = macrometer.calculate_distance(point_cloud, (masks_centroid_left[0]['centroid'], masks_centroid_left[1]['centroid']))
    print('result:',distance)


if __name__ == '__main__':
    config_file_path = r'E:\works\binocular\binocular_distance_measurement-master\config\config.yaml'
    config = yaml.load(open(config_file_path, 'r', encoding='utf-8'), Loader=yaml.Loader)
    img_channels = config['img_channels']
    block_size = config['SGBM']['left_param']['blockSize']
    config['SGBM']['left_param']['P1'] = eval(config['SGBM']['left_param']['P1'])
    config['SGBM']['left_param']['P2'] = eval(config['SGBM']['left_param']['P2'])
    config['SGBM']['right_param']['P1'] = eval(config['SGBM']['right_param']['P1'])
    config['SGBM']['right_param']['P2'] = eval(config['SGBM']['right_param']['P2'])
    config['SGBM']['left_param']['mode'] = eval(config['SGBM']['left_param']['mode'])
    config['SGBM']['right_param']['mode'] = eval(config['SGBM']['right_param']['mode'])
    main(config)
